{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "gQZtf4ZqM8HL"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 4a</h4>\n",
    "<font color=blue><h1>Dynamics and State Feedback Control of a Predator-Prey Model</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1yMOSRNDDNtm-TJGMXX3NS7F4XybOuch-)\n",
    "\n",
    "In this lecture we describe the use of state space control concepts to analyze and stabilize the dynamics of a nonlinear model of a predator-prey system.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "qMVGK15gNQw2"
   },
   "source": [
    "## Predator-Prey System Model\n",
    "\n",
    "We consider a predator-prey system, in which a predator species (lynxes) interacts with a prey species (hares):\n",
    "\n",
    "<center>\n",
    "    <img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/predprey-photo.png\" alt=\"predprey-photo\" width=400>\n",
    "    &nbsp;&nbsp;\n",
    "    <img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/predprey-graph.png\" alt=\"predprey-photo\" width=480>\n",
    "</center>\n",
    "\n",
    "The graph on the right shows the populations of hares and lynxes between 1845 and 1935 in a section of the Canadian Rockies (MacLulich, 1937)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the dynamics for the predator-prey system (no input)\n",
    "predprey_params = {'r': 1.6, 'd': 0.56, 'b': 0.6, 'k': 125, 'a': 3.2, 'c': 50}\n",
    "def predprey_update(t, x, u, params):\n",
    "    \"\"\"Predator prey dynamics\"\"\"\n",
    "    r, d, b, k, a, c = map(params.get, ['r', 'd', 'b', 'k', 'a', 'c'])\n",
    "    u = np.clip(u, -r, r)\n",
    "\n",
    "    # Dynamics for the system\n",
    "    dx0 = (r + u[0]) * x[0] * (1 - x[0]/k) - a * x[1] * x[0]/(c + x[0])\n",
    "    dx1 = b * a * x[1] * x[0] / (c + x[0]) - d * x[1]\n",
    "\n",
    "    return np.array([dx0, dx1])\n",
    "\n",
    "# Create a nonlinear I/O system\n",
    "predprey = ct.nlsys(\n",
    "    predprey_update, name='predprey', params=predprey_params,\n",
    "    states=['H', 'L'], inputs='u', outputs=['H', 'L'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "YmH87LEXWo1U"
   },
   "source": [
    "### Open loop dynamics\n",
    "\n",
    "The open loop dynamics of the system are oscillatory, with a period similar to the data shown above:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = np.linspace(0, 100, 500)\n",
    "response = ct.input_output_response(\n",
    "    predprey, T, 0, [35, 35]\n",
    ")\n",
    "ct.time_response_plot(response, plot_inputs=False, overlay_signals=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also visualize the data using a phase plane plot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a simple phase portrait\n",
    "ct.phase_plane_plot(predprey, [0, 120, 0, 100], 1, gridtype='meshgrid');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We see that the default parameters give a lot of warning messages and the phase portrait does not convey all of the details in some regions of the state space.\n",
    "\n",
    "We can make sure of some of the functions in the `phaseplot` module to get a better view of the dynamics:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a phase portrait\n",
    "ct.phaseplot.equilpoints(predprey, [-5, 126, -5, 100])\n",
    "ct.phaseplot.streamlines(\n",
    "    predprey, np.array([\n",
    "        [0, 100], [1, 0],\n",
    "    ]), 10, color='b')\n",
    "ct.phaseplot.streamlines(\n",
    "    predprey, np.array([[124, 1]]), np.linspace(0, 10, 500), color='b')\n",
    "ct.phaseplot.streamlines(\n",
    "    predprey, np.array([[125, 25], [125, 50], [125, 75]]), 3, color='b')\n",
    "ct.phaseplot.streamlines(predprey, np.array([2, 8]), 6, color='b')\n",
    "ct.phaseplot.streamlines(\n",
    "    predprey, np.array([[20, 30]]), np.linspace(0, 65, 500),\n",
    "    gridtype='circlegrid', gridspec=[2, 1], arrows=10, color='r')\n",
    "ct.phaseplot.vectorfield(predprey, [5, 125, 5, 100], gridspec=[20, 20])\n",
    "\n",
    "# Add the limit cycle\n",
    "resp1 = ct.initial_response(predprey, np.linspace(0, 100), [20, 75])\n",
    "resp2 = ct.initial_response(\n",
    "    predprey, np.linspace(0, 20, 500), resp1.states[:, -1])\n",
    "plt.plot(resp2.states[0], resp2.states[1], color='k');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "KhjlC1258qff"
   },
   "source": [
    "### Find the equilibrium points and check stability\n",
    "\n",
    "We see that there are three equilibrium points in the system.  We can test the stability of the center equilibrium point, which from the phase portrait appears to be unstable."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xe, ue = ct.find_eqpt(predprey, [20, 30], 0)\n",
    "print(f\"{xe=}\")\n",
    "print(f\"{ue=}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sys = predprey.linearize(xe, ue)\n",
    "print(sys)\n",
    "print(\"Poles: \", sys.poles())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "sUECx0cz9QpK"
   },
   "source": [
    "## Stabilization\n",
    "\n",
    "Suppose now that we have the ability to modulate the food supply for the hares.  We do this by modifying the parameter $r$ in the model (this is the term `u` in the model at the top of the notebook).  We can use the `place` command to find a set of gains that stabilize the dynamics around the unstable equilibrium point."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "K = ct.place(sys.A, sys.B, [-0.1, -0.2])\n",
    "print(f\"{K=}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Design an eigenvalue placement (EP) controller to stabilize the equilibrium point\n",
    "epctrl = ct.nlsys(\n",
    "    None, lambda t, x, u, params: -K @ (u[0:2] - xe),\n",
    "    inputs=['H', 'L', 'r'], outputs=['u'],\n",
    ")\n",
    "predprey_ep = ct.interconnect(\n",
    "    [predprey, epctrl], inputs=['r'], outputs=['H', 'L', 'u'],\n",
    "    name='predprey w/ eval placement'\n",
    ")\n",
    "print(predprey_ep)\n",
    "\n",
    "# Show the connection table, useful for debugging what is connected to what\n",
    "predprey_ep.connection_table()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xe_ep, ue_ep = ct.find_eqpt(predprey_ep, [20, 30], [0])\n",
    "print(f\"{xe_ep=}\")\n",
    "print(f\"{ue_ep=}\")\n",
    "print(\"Poles: \", predprey_ep.linearize(xe_ep, ue_ep).poles())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a simple phase portrait\n",
    "ct.phase_plane_plot(\n",
    "    predprey_ep, [0, 120, 0, 100], 1,\n",
    "    plot_separatrices=False,\n",
    "    gridtype='meshgrid', gridspec=[8, 5]\n",
    "    );\n",
    "ct.phaseplot.streamlines(\n",
    "    predprey_ep, np.array([xe_ep]), 20, dir='reverse',\n",
    "    gridtype='circlegrid', gridspec=[4, 11]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simulation from someplace nearby\n",
    "T = np.linspace(0, 40)\n",
    "response = ct.input_output_response(predprey_ep, T, 0, [35, 35])\n",
    "ct.time_response_plot(\n",
    "    response, plot_inputs=False, overlay_signals=True,\n",
    "    title=\"I/O response with eval placement, \" +\n",
    "    f\"r = {predprey.params['r']}\",\n",
    "    legend_loc='upper right')\n",
    "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "zZTBWhlTgSNk"
   },
   "source": [
    "## Integral feedback\n",
    "\n",
    "Another technique that we will learn about later in the class is integral feedback, which can be used to compensate for modeling uncertainty and constant disturbances.\n",
    "\n",
    "We start by asking what happens if we change the value for the parameter $r$ from its original value of 1.6 to a new value of 1.65 (a change of less than 4%):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simulate with a change in food for the hares\n",
    "T = np.linspace(0, 40)\n",
    "response = ct.input_output_response(\n",
    "    predprey_ep, T, 0, [35, 35], params={'r': 1.65}\n",
    ")\n",
    "ct.time_response_plot(\n",
    "    response, plot_inputs=False, overlay_signals=True,\n",
    "    title=\"I/O response w/ eval placement, \" +\n",
    "    f\"r = {response.params['r']}\")\n",
    "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')\n",
    "response.sysname"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We see that the controller no longer stabilizes the equilibrium point (shown with the dashed lines).  In particular, the steady state value of the lynx population does to almost twice the original value.\n",
    "\n",
    "This effect is even worse if we increase $r$ just a bit more (from 1.65 to 1.7)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = np.linspace(0, 40)\n",
    "response = ct.input_output_response(\n",
    "    predprey_ep, T, 0, xe, params={'r': 1.7}\n",
    ")\n",
    "ct.time_response_plot(\n",
    "    response, plot_inputs=False, overlay_signals=True,\n",
    "    title=\"I/O response for predprey w/ eval placement, \" +\n",
    "    f\"r = {response.params['r']}\")\n",
    "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')\n",
    "response.sysname"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The system dynamics are now oscillatory, indicating that we are no longer stabilizing the desired equilibrium point.  This indicates a lack of robustness in our feedback control system.\n",
    "\n",
    "We can compensate for the change in the parameter $r$ by making use of integral feedback in our controller.  We will learn more about integral feedback in later lectures, but for now we demonstrate its ability to compensate for errors in our system model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Integral feedback\n",
    "# Design an eigenvalue placement (EP) controller to stabilize the equilibrium point\n",
    "Ki = 0.0001\n",
    "pictrl = ct.nlsys(\n",
    "    lambda t, x, u, params: u[1] - u[2],\n",
    "    lambda t, x, u, params: -K @ (u[0:2] - xe) - Ki * x[0],\n",
    "    inputs=['H', 'L', 'r'], outputs=['u'], states=1,\n",
    ")\n",
    "predprey_pi = ct.interconnect(\n",
    "    [predprey, pictrl], inputs=['r'], outputs=['H', 'L', 'u'],\n",
    "    name='predprey_pi'\n",
    ")\n",
    "print(predprey_pi)\n",
    "\n",
    "# Simulate with a change in food for the hares\n",
    "T = np.linspace(0, 100, 500)\n",
    "response = ct.input_output_response(\n",
    "    predprey_pi, T, xe[1], [25, 25, 0], params={'r': 1.65})\n",
    "ct.time_response_plot(\n",
    "    response, plot_inputs=False, overlay_signals=True,\n",
    "    title=\"I/O response w/ integral action, \" +\n",
    "    f\"r = {response.params['r']}\",\n",
    "    legend_loc='upper right')\n",
    "\n",
    "plt.plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[0], xe_ep[0]], 'k--')\n",
    "plt.plot([T[0], T[-1]], [xe_ep[1], xe_ep[1]], 'k--')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We see that the system is once again stable at the desired equilibrium point!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
